#ifndef IMU_SENSOR_HPP_
#define IMU_SENSOR_HPP_

#include<stdio.h>
#include<stdlib.h>
#include<fcntl.h>
#include<unistd.h>
#include<assert.h>
#include<termios.h>
#include<string.h>
#include<sys/time.h>
#include<time.h>
#include<sys/types.h>
#include<errno.h>

#include "rclcpp/rclcpp.hpp"

#define BAUD 115200

class WT61C
{
private:
    int fd;
    char r_buf[1024];
    int ret;
public:
    WT61C(/* args */);
    ~WT61C();
    float a[3],w[3],Angle[3],h[3];
    void uart_open(const char *pathname);
    void uart_set(int nSpeed, int nBits, char nEvent, int nStop);
    int uart_close();
    int send_data(char *send_buffer,int length);
    int recv_data(int length);
    void ParseData(char chr);
    void update();
};


WT61C::WT61C(/* args */)
{
    bzero(r_buf,1024);
    uart_open("/dev/ttyUSB0");
    uart_set(BAUD,8,'N',1);
}

WT61C::~WT61C()
{
    ret = uart_close();
    if(ret == -1)
    {
      RCLCPP_FATAL(rclcpp::get_logger("IMU_SENSOR"), "UNART close error!");
      exit(EXIT_FAILURE);
    }
    RCLCPP_INFO(rclcpp::get_logger("IMU_SENSOR"), "UART exit successed.");
    exit(EXIT_SUCCESS);
}

void WT61C::uart_open(const char *pathname)
{
    fd = open(pathname, O_RDWR|O_NOCTTY); 
    if (-1 == fd)
    { 
      RCLCPP_FATAL(rclcpp::get_logger("IMU_SENSOR"), "Can't Open Serial Port.");
		return; 
	  } 
    else
		RCLCPP_INFO(rclcpp::get_logger("IMU_SENSOR"), "open %s success!",pathname);
    if(isatty(STDIN_FILENO)==0) 
		RCLCPP_INFO(rclcpp::get_logger("IMU_SENSOR"), "standard input is not a terminal device"); 
    else 
		RCLCPP_INFO(rclcpp::get_logger("IMU_SENSOR"), "isatty success!"); 

    if(fd == -1)
    {
        RCLCPP_FATAL(rclcpp::get_logger("IMU_SENSOR"), "uart_open error");
        exit(EXIT_FAILURE);
    }
}

void WT61C::uart_set(int nSpeed, int nBits, char nEvent, int nStop)
{
     struct termios newtio,oldtio; 
     if  ( tcgetattr(fd, &oldtio)  !=  0) {  
      perror("SetupSerial 1");
	  printf("tcgetattr( fd,&oldtio) -> %d\n",tcgetattr(fd, &oldtio)); 
      return; 
     } 
     bzero( &newtio, sizeof( newtio ) ); 
     newtio.c_cflag  |=  CLOCAL | CREAD;  
     newtio.c_cflag &= ~CSIZE;  
     switch( nBits ) 
     { 
     case 7: 
      newtio.c_cflag |= CS7; 
      break; 
     case 8: 
      newtio.c_cflag |= CS8; 
      break; 
     } 
     switch( nEvent ) 
     { 
     case 'o':
     case 'O': 
      newtio.c_cflag |= PARENB; 
      newtio.c_cflag |= PARODD; 
      newtio.c_iflag |= (INPCK | ISTRIP); 
      break; 
     case 'e':
     case 'E': 
      newtio.c_iflag |= (INPCK | ISTRIP); 
      newtio.c_cflag |= PARENB; 
      newtio.c_cflag &= ~PARODD; 
      break;
     case 'n':
     case 'N': 
      newtio.c_cflag &= ~PARENB; 
      break;
     default:
      break;
     } 

     /*设置波特率*/ 

switch( nSpeed ) 
     { 
     case 2400: 
      cfsetispeed(&newtio, B2400); 
      cfsetospeed(&newtio, B2400); 
      break; 
     case 4800: 
      cfsetispeed(&newtio, B4800); 
      cfsetospeed(&newtio, B4800); 
      break; 
     case 9600: 
      cfsetispeed(&newtio, B9600); 
      cfsetospeed(&newtio, B9600); 
      break; 
     case 115200: 
      cfsetispeed(&newtio, B115200); 
      cfsetospeed(&newtio, B115200); 
      break; 
     case 460800: 
      cfsetispeed(&newtio, B460800); 
      cfsetospeed(&newtio, B460800); 
      break; 
     default: 
      cfsetispeed(&newtio, B9600); 
      cfsetospeed(&newtio, B9600); 
     break; 
     } 
     if( nStop == 1 ) 
      newtio.c_cflag &=  ~CSTOPB; 
     else if ( nStop == 2 ) 
      newtio.c_cflag |=  CSTOPB; 
     newtio.c_cc[VTIME]  = 0; 
     newtio.c_cc[VMIN] = 0; 
     tcflush(fd,TCIFLUSH); 

if((tcsetattr(fd,TCSANOW,&newtio))!=0) 
     { 
      RCLCPP_FATAL(rclcpp::get_logger("IMU_SENSOR"), "com set error"); 
      return; 
     } 
     RCLCPP_INFO(rclcpp::get_logger("IMU_SENSOR"), "UART set done!\n"); 
     return; 
}

int WT61C::uart_close()
{
    assert(fd);
    close(fd);

    return 0;
}

int WT61C::send_data(char *send_buffer,int length)
{
	length=write(fd,send_buffer,length*sizeof(unsigned char));
	return length;
}

int WT61C::recv_data(int length)
{
	ret = read(fd,r_buf,length);
  if(ret == -1)
  {
    RCLCPP_FATAL(rclcpp::get_logger("IMU_SENSOR"), "uart read failed!\n");
    exit(EXIT_FAILURE);
  }
  return ret;
}

void WT61C::ParseData(char chr)

{
    static char chrBuf[100];
    static unsigned char chrCnt=0;
    signed short sData[4];
    unsigned char i;
    
    time_t now;

    
    chrBuf[chrCnt++]=chr;
    if (chrCnt<11) return;
    
    if ((chrBuf[0]!=0x55)||((chrBuf[1]&0x50)!=0x50)) 
    {
        //printf("Error:%x %x\r\n",chrBuf[0],chrBuf[1]);
        memcpy(&chrBuf[0],&chrBuf[1],10);
        chrCnt--;
        return;
    }
    
    memcpy(&sData[0],&chrBuf[2],8);
		switch(chrBuf[1])
		{
				case 0x51:
					for (i=0;i<3;i++) a[i] = (float)sData[i]/32768.0*16.0;
					time(&now);
					//printf("\r\nT:%s a:%6.3f %6.3f %6.3f ",asctime(localtime(&now)),a[0],a[1],a[2]);
					
					break;
				case 0x52:
					for (i=0;i<3;i++) w[i] = (float)sData[i]/32768.0*2000.0;
					//printf("w:%7.3f %7.3f %7.3f ",w[0],w[1],w[2]);					
					break;
				case 0x53:
					for (i=0;i<3;i++) Angle[i] = (float)sData[i]/32768.0*180.0;
					//printf("A:%7.3f %7.3f %7.3f ",Angle[0],Angle[1],Angle[2]);
					break;
				case 0x54:
					for (i=0;i<3;i++) h[i] = (float)sData[i];
					//printf("h:%4.0f %4.0f %4.0f ",h[0],h[1],h[2]);
					
					break;
		}		
		chrCnt=0;		
}

void WT61C::update()
{
  recv_data(44);
	for (int i=0;i<ret;i++) 
  {
    ParseData(r_buf[i]);
  }
  return;
}

#endif
